#!/bin/bash

echo "Starting can..."
sudo modprobe gs_usb 
sudo ip link set can0 up type can bitrate 500000
echo "---------------------------can started---------------------------"
sleep 2
# 启动 roscore
echo "Starting roscore..."
roscore &
ROSCORE_PID=$!
echo "---------------------------roscore started---------------------------"
sleep 5

# 启动激光雷达
echo "Starting LiDAR..."
roslaunch scout_bringup open_rslidar.launch > /dev/null 2>&1 &
LIDAR_PID=$!
sleep 5

# 启动相机
echo "Starting camera..."
roslaunch realsense2_camera rs_aligned_depth.launch > /dev/null 2>&1 &
CAMERA_PID=$!
sleep 5

# 启动RTAB
echo "Starting RTAB..."
roslaunch scout_bringup rtabmapping.launch localization:=true &
RTAB_PID=$!
sleep 5

# 启动MOVEBASE
echo "Starting MOVE_BASE..."
roslaunch scout_bringup scout_rtab_navigation.launch &
MOVE_BASE_PID=$!
sleep 5

# 启动识别二维码节点
source ./catkin_ws/devel/setup.bash
echo "Starting sia_camera..."
roslaunch sia_camera task4.launch &
SIA_CAMERA_PID=$!
sleep 5


echo "---------------------------car's started---------------------------"

# 定义停止所有进程的函数
cleanup() {
    echo -e '\n---------------------------Stopping---------------------------\n'
    kill -TERM $ROSCORE_PID $LIDAR_PID $CAMERA_PID $RTAB_PID $MOVE_BASE_PID $SIA_CAMERA_PID
    wait $ROSCORE_PID $LIDAR_PID $CAMERA_PID $RTAB_PID $MOVE_BASE_PID $SIA_CAMERA_PID
    echo -e '\n---------------------------All processes stopped---------------------------\n'
    exit
}

# 捕捉 Ctrl+C 信号以便停止所有进程
trap cleanup SIGINT

# 保持脚本运行
while true; do
    sleep 1
done